/*********************************************************************
*
*   CppBot.cpp - C++ Robot simulator
*
*   Copyright:
*       Software source code by Alex Morozov and Chris D. Locke is
*       licensed under a Creative Commons Attribution-Noncommercial-
*       Share Alike 3.0 United States License
*       (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)
*
*********************************************************************/

#include "stdafx.h"

#include <winsock2.h>

#include <windows.h>

#include <crtdbg.h>
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stddef.h>

#include "glut.h"

#include "CppBot_types.h"

#include "utl_draw.h"
#include "ROB_pub.h"
#include "RobotMath.h"


/*----------------------------------------------------------
Literal constants
----------------------------------------------------------*/
#define PRDC_RATE_MS ( 5 )
#define JOY_W        ( 100 )
#define JOY_H        ( 100 )
#define MAX_OBJECTS  ( 1024 )
#define MAX_POINTS   ( 10 )
#define MAX_SLOTS    ( 8 )
#define MAX_DIO_CHAN ( 16 )

/*----------------------------------------------------------
Types
----------------------------------------------------------*/
#pragma pack( push )
#pragma pack( 1 )
struct ds_data_type
    {
    uint16              pkt_idx;
    uint8               dig_in;
    uint8               dig_out;
    uint16              batt;
    uint8               status;
    uint8               err;
    uint16              team_id;
    uint8               ver_mon1;
    uint8               ver_mon2;
    uint8               ver_day1;
    uint8               ver_day2;
    uint8               ver_year1;
    uint8               ver_year2;
    uint8               rev1;
    uint8               rev2;
    uint32              unused1;
    uint16              unused2;
    uint16              reply_pkt_num;
    };

struct dig_mod_data_type
    {
    uint8               relay_fw;
    uint8               relay_rev;
    uint16              gpio;
    uint16              gpio_oe;
    uint8               pwm1[ 10 ];
    };

struct bot_data_type
    {
    float               analog0[ 8 ];
    float               analog1[ 8 ];
    dig_mod_data_type   dig_mod_0;
    dig_mod_data_type   dig_mod_1;
    uint8               sol;
    };   
    
struct dash_pkt_type
    {
    ds_data_type        ds;
    bot_data_type       bot;
    uint32              crc;
    };     
#pragma pack( pop )    


struct x_y_type
    {
    float               x, y;
    };
    
enum
    {
    OBJ_TYPE_POLY,
    OBJ_TYPE_CIRCLE
    };    

struct field_object_type
    {
    int                 obj_type;
    float               r, g, b;
    int                 radius;
    int                 ct;
    x_y_type            xy[ MAX_POINTS ];
    };

/*----------------------------------------------------------
Variables
----------------------------------------------------------*/
static dash_pkt_type    dash_pkt;
static uint16           dio[ MAX_SLOTS ]; 
static uint16           dio_oe[ MAX_SLOTS ];
static int              field_obj_ct = 0;
static field_object_type
                        obj[ MAX_OBJECTS ];
static boolean          output=TRUE;/* debug output                 */
static boolean          running = TRUE;
static char             str[ 1024 ];/* string variable              */
GLuint                  field_list;
static unsigned int     time_ms = 0;
static struct sockaddr_in      
                        to_addr;
static WSAOVERLAPPED    tx_ol;
static SOCKET           tx_sock = INVALID_SOCKET;
static int              window_h=1; /* window height                */
static int              window_w=1; /* window width                 */
static int              joy_x1, joy_x2, joy_y1, joy_y2;
static int              joy_x = 0, joy_y = 0;

/*----------------------------------------------------------
Procedures
----------------------------------------------------------*/
static void display_cb              /* display callback             */
    ( void );
static void idle_cb                 /* idle callback               */
    ( void );
static void keyboard_cb             /* keyboard callback            */
    (
    unsigned char       key,        /* key pressed                  */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    );
static void motion_cb               /* motion callback              */
    (
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    );
static void mouse_cb                /* mouse callback               */
    (
    int                 button,     /* mouse button                 */
    int                 state,      /* button state                 */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    );
static void passive_cb              /* passive callback             */
    (
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    );
static void print_help
    ( void );
static void reshape_cb              /* reshape callback             */
    (
    int                 x,          /* new x dimension              */
    int                 y           /* new y demension              */
    );
static void special_up_cb           /* keyboard callback            */
    (
    int                 key,        /* key pressed                  */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    );
static void special_cb             /* keyboard callback            */
    (
    int                 key,        /* key pressed                  */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    );
static void CALLBACK time_proc
    (
    UINT uID,
    UINT uMsg,
    DWORD dwUser,
    DWORD dw1,
    DWORD dw2
    );
static void CALLBACK tx_completion
    (
    DWORD               dwError,
    DWORD               cbTransferred,
    LPWSAOVERLAPPED     lpOverlapped,
    DWORD               dwFlags
    );


/*********************************************************************
*
*       main - Main function
*
*********************************************************************/

int main(int argc, char* argv[])
{

#define DASH_PORT ( 1165 )

struct hostent *        hp;
WSADATA                 wsaData;

/*----------------------------------------------------------
Init device connection and dashboard packet
----------------------------------------------------------*/
if( WSAStartup( 0x202, &wsaData ) == SOCKET_ERROR )
    {
    WSACleanup( );
    _ASSERT( 0 );
    }
memset( &dash_pkt, 0, sizeof( dash_pkt ) );

/*----------------------------------------------------------
Change the string below to localhost if dashboard is running
on same machine or IP address of dashboard machine
----------------------------------------------------------*/
#if 0
#define _HOST "192.168.1.101"
#else
#define _HOST "localhost"
#endif
hp = gethostbyname( _HOST );
if( hp == NULL )
    {
    int err = WSAGetLastError( );
    _ASSERT( 0 );
    }
tx_sock = WSASocket( AF_INET, SOCK_DGRAM, IPPROTO_UDP, NULL, 0, WSA_FLAG_OVERLAPPED );
_ASSERT( tx_sock != INVALID_SOCKET );
to_addr.sin_family      = AF_INET;
to_addr.sin_addr.s_addr = INADDR_ANY;
to_addr.sin_port = htons( DASH_PORT );
memcpy( &( to_addr.sin_addr ), hp->h_addr, hp->h_length );

/*----------------------------------------------------------
Init dashboard packet
----------------------------------------------------------*/
dash_pkt.ds.batt    = ntohs( 0x1200 );
dash_pkt.ds.status  = 0xff;
dash_pkt.ds.team_id = ntohs( ( uint16 )( ( 23 << 8 ) | 34 ) );

/*----------------------------------------------------------
Read in field objects
----------------------------------------------------------*/
FILE *                  fp = argc > 1 ? fopen( argv[ 1 ], "rt" ) : NULL;

if( fp != NULL )
    {
    float               r, g, b;
    int                 x, y, rad;
    char                buf[ 500 ];
    while( fgets( buf, sizeof( buf ), fp ) )
        {

        /*--------------------------------------------------
            Input syntax:
        color 1, 0, 0
        point 100, 100
        --------------------------------------------------*/
        if( 3 == sscanf( buf, "color %f, %f, %f", &r, &g, &b ) )
            {
            field_obj_ct++;
            _ASSERT( field_obj_ct < MAX_OBJECTS );
            obj[ field_obj_ct - 1 ].ct = 0;
            obj[ field_obj_ct - 1 ].r = r;
            obj[ field_obj_ct - 1 ].g = g;
            obj[ field_obj_ct - 1 ].b = b;
            }
        else if( 2 == sscanf( buf, "point %d, %d", &x, &y ) )
            {
            obj[ field_obj_ct - 1 ].obj_type = OBJ_TYPE_POLY;
            obj[ field_obj_ct - 1 ].xy[ obj[ field_obj_ct - 1 ].ct ].x = x;
            obj[ field_obj_ct - 1 ].xy[ obj[ field_obj_ct - 1 ].ct ].y = y;
            obj[ field_obj_ct - 1 ].ct++;
            }
        else if( 3 == sscanf( buf, "circle %d, %d, %d", &x, &y, &rad ) )
            {
            obj[ field_obj_ct - 1 ].obj_type = OBJ_TYPE_CIRCLE;
            obj[ field_obj_ct - 1 ].xy[ 0 ].x = x;
            obj[ field_obj_ct - 1 ].xy[ 0 ].y = y;
            obj[ field_obj_ct - 1 ].radius = rad;
            }
        }
    fclose( fp );
    }

/*----------------------------------------------------------
Print help screeen on console window
----------------------------------------------------------*/
print_help( );

/*----------------------------------------------------------
Set the Windows time period to 5ms resolution (200Hz)
----------------------------------------------------------*/
timeBeginPeriod( 1 );
timeSetEvent( PRDC_RATE_MS, 0, &time_proc, 0, TIME_PERIODIC );

/*----------------------------------------------------------
Initialize GLUT and create window
----------------------------------------------------------*/
glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH );
glutInitWindowPosition( 50, 50 );
glutInitWindowSize( 600, 600 );
glutCreateWindow( "CppBot" );

/*----------------------------------------------------------
Create display list
----------------------------------------------------------*/
int i, j;
field_list = glGenLists( 1 );
glNewList( field_list, GL_COMPILE );
for( i = 0; i < field_obj_ct; i++ )
    {
    glColor3f( obj[ i ].r, obj[ i ].g, obj[ i ].b );
    if( obj[ i ].obj_type == OBJ_TYPE_POLY )
        {
        glBegin( GL_POLYGON );
            {
            for( j = 0; j < obj[ i ].ct; j++ )
                {
                glVertex2f( obj[ i ].xy[ j ].x, obj[ i ].xy[ j ].y );
                }
            }
        glEnd();
        }
    else if( obj[ i ].obj_type == OBJ_TYPE_CIRCLE )
        {
        glBegin( GL_POLYGON );
            {
            #define CIRCLE_SLICES 20
		    for (int j = 1; j < 360; j += 360 / CIRCLE_SLICES )
		        {
			    double heading = ( double )j * PI / 180.0;
			    glVertex2d
			        ( 
			        obj[ i ].xy[ 0 ].x + cos( heading ) * obj[ i ].radius, 
			        obj[ i ].xy[ 0 ].y + sin( heading ) * obj[ i ].radius 
			        );
			    }
		    }
        glEnd();
        }
    }
glEndList();

/*----------------------------------------------------------
Register callbacks
----------------------------------------------------------*/
glutIdleFunc( idle_cb );
glutReshapeFunc( reshape_cb );
glutDisplayFunc( display_cb );
glutKeyboardFunc( keyboard_cb );
glutSpecialFunc( special_cb );
glutSpecialUpFunc( special_up_cb );
glutMouseFunc( mouse_cb );
glutMotionFunc( motion_cb );
glutPassiveMotionFunc( passive_cb );

/*----------------------------------------------------------
Initialize helper draw functions
----------------------------------------------------------*/
cdp_utl_draw_pwrp();

/*----------------------------------------------------------
Run main program
----------------------------------------------------------*/
glutMainLoop();

/*----------------------------------------------------------
Clear the Windows time period from 1ms resolution
----------------------------------------------------------*/
timeEndPeriod(1);

return( 0 );

}   /* main( ) */


/*********************************************************************
*
*   get_dio - Return digital input state
*
*********************************************************************/

bool get_dio
    (
    int                 slot,
    int                 chan
    )
{
return( ( dio[ slot ] & ( 1 << ( chan - 1 ) ) ) ? true : false );

}   /* get_dio( ) */


/*********************************************************************
*
*   get_joy - Return joystick position
*
*********************************************************************/

void get_joy
    (
    int                 idx,
    int *               x,
    int *               y
    )
{
int                     val_x = 0, val_y = 0;
if( idx == 0 )
    {
    val_x = joy_x * 2;
    val_y = joy_y * 2;
    }

if( x != NULL )
    {
    *x = val_x;
    }
if( y != NULL )
    {
    *y = val_y;
    }

}   /* get_joy( ) */


/*********************************************************************
*
*   init_dio - Init digital input OR output
*
*********************************************************************/

void init_dio
    (
    int                 slot,
    int                 chan,
    bool                output
    )
{

//Slots are numbered 1-8
_ASSERT( ( slot >= 1 ) && ( slot <= MAX_SLOTS ) );

//Channels are numbered 1-8
_ASSERT( ( chan >= 1 ) && ( chan <= MAX_DIO_CHAN ) );

if( output )
    {
    dio_oe[ slot ] |= ( 1 << ( chan - 1 )  );
    }
else
    {
    dio_oe[ slot ] &= ~( 1 << ( chan - 1 )  );
    }

}   /* init_dio( ) */


/*********************************************************************
*
*   set_dio - Set digital output state
*
*********************************************************************/

void set_dio
    (
    int                 slot,
    int                 chan,
    bool                val
    )
{
//Slots are numbered 1-8
_ASSERT( ( slot >= 1 ) && ( slot <= MAX_SLOTS ) );

//Channels are numbered 1-8
_ASSERT( ( chan >= 1 ) && ( chan <= MAX_DIO_CHAN ) );

if( val )
    {
    dio[ slot ] |= ( 1 << ( chan - 1 )  );
    }
else
    {
    dio[ slot ] &= ~( 1 << ( chan - 1 )  );
    }

}   /* set_dio( ) */


/*********************************************************************
*
*   display_cb - Draw the graphics
*
*********************************************************************/

static void display_cb              /* display callback             */
    ( void )
{

/*----------------------------------------------------------
Local literal constants
----------------------------------------------------------*/

/*----------------------------------------------------------
Local variables
----------------------------------------------------------*/
int                 i;
float               max_mag_x = ( float )window_w / 2.0f;
float               max_mag_y = ( float )window_h / 2.0f;

/*----------------------------------------------------------
Clear screen
----------------------------------------------------------*/
glClear( GL_COLOR_BUFFER_BIT );

/*----------------------------------------------------------
Load identity matrix
----------------------------------------------------------*/
glMatrixMode( GL_MODELVIEW );
glLoadIdentity();

/*----------------------------------------------------------
Draw bot
----------------------------------------------------------*/
glTranslatef( max_mag_x, max_mag_y, 0.0f );
glScalef( 1.0f, -1.0f, 1.0f );

/*----------------------------------------------------------
Draw grid
----------------------------------------------------------*/
glPushMatrix( );
glLineWidth( 3.0f );
glBegin( GL_LINES );
    glColor3f( 0.3f, 0.3f, 0.3f );
    glVertex2f( -20.0f, 0.0f );
    glVertex2f( 20.0f, 0.0f );

    glVertex2f( 0.0f, -20.0f );
    glVertex2f( 0.0f, 20.0f );
glEnd();
glLineWidth( 1.0f );
for( i = 0; i < window_w / 2; i += 100 )
    {
    glBegin( GL_LINES );
        glColor3f( 0.3f, 0.3f, 0.3f );
        glVertex2f( ( float )i, -max_mag_y );
        glVertex2f( ( float )i, max_mag_y );
    glEnd();
    }
for( i = -100; i > -window_w / 2; i -= 100 )
    {
    glBegin( GL_LINES );
        glColor3f( 0.3f, 0.3f, 0.3f );
        glVertex2f( ( float )i, -max_mag_y );
        glVertex2f( ( float )i, max_mag_y );
    glEnd();
    }

for( i = 0; i < window_h / 2; i += 100 )
    {
    glBegin( GL_LINES );
        glColor3f( 0.3f, 0.3f, 0.3f );
        glVertex2f( -max_mag_x, ( float )i );
        glVertex2f( max_mag_x, ( float )i );
    glEnd();
    }

for( i = -100; i > -window_h / 2; i -= 100 )
    {
    glBegin( GL_LINES );
        glColor3f( 0.3f, 0.3f, 0.3f );
        glVertex2f( -max_mag_x, ( float )i );
        glVertex2f( max_mag_x, ( float )i );
    glEnd();
    }
glPopMatrix( );

/*----------------------------------------------------------
Draw field objects
----------------------------------------------------------*/
glPushMatrix( );
    {
    glCallList( field_list );
    }
glPopMatrix( );

/*----------------------------------------------------------
Draw joystick
----------------------------------------------------------*/
glPushMatrix( );
    glTranslatef( 0, -max_mag_y + 50.0f, 0.0f );
    glBegin( GL_QUADS );
        glColor3f( 0.5f, 0.5f, 0.5f );
        glVertex2f( -50.0f, -50.0f );
        glVertex2f( 50.0f, -50.0f );
        glVertex2f( 50.0f, 50.0f );
        glVertex2f( -50.0f, 50.0f );
    glEnd();
    glBegin( GL_LINES );
        glColor3f( 0.0f, 0.0f, 0.0f );
        glVertex2f( -20.0f, 0.0f );
        glVertex2f( 20.0f , 0.0f );
        glVertex2f( 0.0f , -20.0f );
        glVertex2f( 0.0f , 20.0f );
    glEnd();
    glBegin( GL_QUADS );
        glColor3f( 1.0f, 1.0f, 1.0f );
        glVertex2f( ( float )( joy_x - 2 ), ( float )( joy_y - 2 ) );
        glVertex2f( ( float )( joy_x + 2 ), ( float )( joy_y - 2 ) );
        glVertex2f( ( float )( joy_x + 2 ), ( float )( joy_y + 2 ) );
        glVertex2f( ( float )( joy_x - 2 ), ( float )( joy_y + 2 ) );
    glEnd();
glPopMatrix( );


/*----------------------------------------------------------
Draw motor bars
----------------------------------------------------------*/
glPushMatrix( );
    char                buf[ 100 ];
    int                 lt;
    int                 rt;

    rob_get_motors( &rt, &lt );    //expect +/- 100

    glLineWidth( 10.0f );
    glBegin( GL_LINES );
        glColor3f( rt > 0.0f ? 0.0f : 1.0f, rt > 0.0f ? 1.0f : 0.0f, 0.0f );
        glVertex2f( max_mag_x * 0.9f, 0.0f );
        glVertex2f( max_mag_x * 0.9f, max_mag_y * ( float )rt / 100.0f );
    glEnd();

    glPushMatrix( );
        glColor3f( 1.0f, 1.0f, 1.0f );
        glScalef( 1.0f, -1.0f, 1.0f );
        sprintf( buf, "%d", rt );
        cdp_utl_draw_text( max_mag_x * 0.9f, 0.0f, buf );
    glPopMatrix( );

    glBegin( GL_LINES );
        glColor3f( lt > 0.0f ? 0.0f : 1.0f, lt > 0.0f ? 1.0f : 0.0f, 0.0f );
        glVertex2f( max_mag_x * -0.9f, 0.0f );
        glVertex2f( max_mag_x * -0.9f, max_mag_y * ( float )lt / 100.0f );
    glEnd();

    glPushMatrix( );
        glColor3f( 1.0f, 1.0f, 1.0f );
        glScalef( 1.0f, -1.0f, 1.0f );
        sprintf( buf, "%d", lt );
        cdp_utl_draw_text( max_mag_x * -0.9f, 0.0f, buf );
    glPopMatrix( );

glPopMatrix( );


/*----------------------------------------------------------
Draw position and time text
----------------------------------------------------------*/
float               hdg, x, y;
rob_get_pos( &hdg, &x, &y );
glPushMatrix( );
    glColor3f( 1.0f, 1.0f, 1.0f );
    glScalef( 1.0f, -1.0f, 1.0f );
    sprintf( buf, "x=%.2f y=%.2f", x, y );
    cdp_utl_draw_text( max_mag_x * -0.9f, max_mag_y * -0.9f, buf );
    sprintf( buf, "t=%.3f", ( float )time_ms / 1000.0f );
    cdp_utl_draw_text( max_mag_x * -0.9f, max_mag_y * -0.85f, buf );
glPopMatrix( );

/*----------------------------------------------------------
Draw robot 2ft by 3ft - 24 units per foot - 48 x 72
----------------------------------------------------------*/
#define ROBOT_WIDTH  ( 48.0f )
#define ROBOT_HEIGHT ( 72.0f )
#define ROBOT_WIDTH_BY2  ( ROBOT_WIDTH / 2.0f )
#define ROBOT_HEIGHT_BY2 ( ROBOT_HEIGHT / 2.0f )
glPushMatrix( );
    glTranslatef( x, y, 0.0f );
    glRotatef( -hdg, 0.0f, 0.0f, 1.0f );

    glLineWidth( 2.0f );
    glBegin( GL_LINES );
        glColor3f( 1.0f, 1.0f, 1.0f );
        glVertex2f( -ROBOT_WIDTH_BY2, -ROBOT_HEIGHT_BY2 );
        glVertex2f( ROBOT_WIDTH_BY2, -ROBOT_HEIGHT_BY2 );

        glVertex2f( ROBOT_WIDTH_BY2, -ROBOT_HEIGHT_BY2 );
        glVertex2f( ROBOT_WIDTH_BY2, ROBOT_HEIGHT_BY2 );

        glVertex2f( -ROBOT_WIDTH_BY2, ROBOT_HEIGHT_BY2 );
        glVertex2f( -ROBOT_WIDTH_BY2, -ROBOT_HEIGHT_BY2 );

        glColor3f( 1.0f, 0.0f, 0.0f );
        glVertex2f( ROBOT_WIDTH_BY2, ROBOT_HEIGHT_BY2 );
        glVertex2f( -ROBOT_WIDTH_BY2, ROBOT_HEIGHT_BY2 );

    glEnd();
glPopMatrix( );

/*----------------------------------------------------------
Swap buffers to update display
----------------------------------------------------------*/
glutSwapBuffers();

} /* display_cb() */


/*********************************************************************
*
*   idle_cb - Idle callback
*
*********************************************************************/

static void idle_cb                 /* idle callback               */
    ( void )
{

/*----------------------------------------------------------
Update display
----------------------------------------------------------*/
glutPostRedisplay();

/*----------------------------------------------------------
Be nice to the CPU (windows specific Sleep call)
----------------------------------------------------------*/
Sleep( 10 );

} /* idle_cb() */


/*********************************************************************
*
*   keyboard_cb - Glut Keyboard Callback Function
*
*********************************************************************/

static void keyboard_cb             /* keyboard callback            */
    (
    unsigned char       key,        /* key pressed                  */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    )
{

/*----------------------------------------------------------
Process keyboard key presses
----------------------------------------------------------*/
switch( key )
    {

    /*------------------------------------------------------
    Pause simulation
    ------------------------------------------------------*/
    case ' ':
        running = !running;
        break;

    /*------------------------------------------------------
    Exit program if escape is pressed
    ------------------------------------------------------*/
    case 27:
        exit(0);
        break;

    /*------------------------------------------------------
    Default
    ------------------------------------------------------*/
    default:
        break;
    }
}   /* keyboard_cb() */


/*********************************************************************
*
*   motion_cb - Glut Motion Callback Function
*
*********************************************************************/

static void motion_cb               /* motion callback              */
    (
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    )
{
/*----------------------------------------------------------
Convert xy coordinates
----------------------------------------------------------*/
x = x - ( window_w / 2 );
y = y - ( window_h / 2 );

if( ( x >= joy_x1 ) && ( x <= joy_x2 ) )
    {
    if( ( y >= joy_y1 ) && ( y <= joy_y2 ) )
        {
        joy_x = x;
        joy_y = ( joy_y1 + ( JOY_H / 2 ) ) - y;
        }
    }

} /* motion_cb() */


/*********************************************************************
*
*   mouse_cb - Glut Mouse Callback Function
*
*********************************************************************/

static void mouse_cb                /* mouse callback               */
    (
    int                 button,     /* mouse button                 */
    int                 state,      /* button state                 */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    )
{
/*----------------------------------------------------------
Local variables
----------------------------------------------------------*/

/*----------------------------------------------------------
Left mouse button
----------------------------------------------------------*/
if( button == GLUT_LEFT_BUTTON )
    {
    /*------------------------------------------------------
    Mouse up
    ------------------------------------------------------*/
    if( state == GLUT_UP )
        {
        joy_x = 0;
        joy_y = 0;
        }

    /*------------------------------------------------------
    Mouse down
    ------------------------------------------------------*/
    else if( state == GLUT_DOWN )
        {
        }
    }

/*----------------------------------------------------------
Right mouse button
----------------------------------------------------------*/
else if( button == GLUT_RIGHT_BUTTON )
    {
    /*------------------------------------------------------
    Mouse up
    ------------------------------------------------------*/
    if( state == GLUT_UP )
        {
        }

    /*------------------------------------------------------
    Mouse down
    ------------------------------------------------------*/
    else if( state == GLUT_DOWN )
        {
        }
    }
} /* mouse_cb() */


/*********************************************************************
*
*   passive_cb - Glut Passive Motion Callback Function
*
*********************************************************************/

static void passive_cb              /* passive callback             */
    (
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    )
{

/*----------------------------------------------------------
Convert xy coordinates
----------------------------------------------------------*/
x = x / window_w;
y = y / window_h;

} /* passive_cb() */


/*********************************************************************
*
*   print_help - Print help to console screen
*
*********************************************************************/

static void print_help
    ( void )
{
printf( "CppBot version 1.0                                              \n" );
printf( "    Software source code by Alex Morozov and Chris D. Locke is  \n" );
printf( "    licensed under a Creative Commons Attribution-Noncommercial-\n" );
printf( "    Share Alike 3.0 United States License                       \n" );
printf( "    (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)      \n" );
printf( "\nKeyboard usage:                                               \n" );
printf( "    space      - Pause Simulation                               \n" );
printf( "    Esc        - Quit CppBot                                    \n" );
printf( "    F1-F8      - First 8 digital inputs on slot 6               \n" );

}   /* print_help( ) */


/*********************************************************************
*
*   reshape_cb - Reshape callback
*
*********************************************************************/

static void reshape_cb              /* reshape callback             */
    (
    int                 x,          /* new x dimension              */
    int                 y           /* new y demension              */
    )
{

/*----------------------------------------------------------
Create viewport
----------------------------------------------------------*/
glViewport( 0, 0, x, y );

/*----------------------------------------------------------
Save the new width and height
----------------------------------------------------------*/
window_w = x;
window_h = y;

joy_x1 = JOY_W / -2;
joy_x2 = JOY_W / 2;
joy_y2 = window_h / 2;
joy_y1 = joy_y2 - JOY_H;

/*----------------------------------------------------------
Set new Orthographic viewing area
----------------------------------------------------------*/
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
glOrtho( 0.0f, x, y, 0.0f, -1.0f, 1.0f );
//glScalef( ( float )x, ( float )y, 1.0f );
//glScalef( (float)x / (float)SCREEN_W, (float)y / (float)SCREEN_H, 1.0f );

/*----------------------------------------------------------
Redraw display
----------------------------------------------------------*/
glutPostRedisplay();

} /* reshape_cb() */


/*********************************************************************
*
*   special_cb - Glut Special Keyboard Callback Function
*
*********************************************************************/

static void special_up_cb           /* keyboard callback            */
    (
    int                 key,        /* key pressed                  */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    )
{

/*----------------------------------------------------------
Process keyboard key presses
----------------------------------------------------------*/
switch( key )
    {

    case GLUT_KEY_F1:
        if( ( dio_oe[ 6 ] & ( 1 << 0 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 0 );
            }
        break;
    case GLUT_KEY_F2:
        if( ( dio_oe[ 6 ] & ( 1 << 1 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 1 );
            }
        break;
    case GLUT_KEY_F3:
        if( ( dio_oe[ 6 ] & ( 1 << 2 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 2 );
            }
        break;
    case GLUT_KEY_F4:
        if( ( dio_oe[ 6 ] & ( 1 << 3 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 3 );
            }
        break;
    case GLUT_KEY_F5:
        if( ( dio_oe[ 6 ] & ( 1 << 4 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 4 );
            }
        break;
    case GLUT_KEY_F6:
        if( ( dio_oe[ 6 ] & ( 1 << 5 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 5 );
            }
        break;
    case GLUT_KEY_F7:
        if( ( dio_oe[ 6 ] & ( 1 << 6 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 6 );
            }
        break;
    case GLUT_KEY_F8:
        if( ( dio_oe[ 6 ] & ( 1 << 7 ) ) == 0 )
            {
            dio[ 6 ] &= ~( 1 << 7 );
            }
        break;

    /*------------------------------------------------------
    Default
    ------------------------------------------------------*/
    default:
        break;
    }

}   /* special_up_cb( ) */


/*********************************************************************
*
*   special_cb - Glut Special Keyboard Callback Function
*
*********************************************************************/

static void special_cb             /* keyboard callback            */
    (
    int                 key,        /* key pressed                  */
    int                 x,          /* mouse X coordinate           */
    int                 y           /* mouse Y coordinate           */
    )
{

/*----------------------------------------------------------
Process keyboard key presses
----------------------------------------------------------*/
switch( key )
    {

    case GLUT_KEY_F1:
        if( ( dio_oe[ 6 ] & ( 1 << 0 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 0 );
            }
        break;
    case GLUT_KEY_F2:
        if( ( dio_oe[ 6 ] & ( 1 << 1 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 1 );
            }
        break;
    case GLUT_KEY_F3:
        if( ( dio_oe[ 6 ] & ( 1 << 2 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 2 );
            }
        break;
    case GLUT_KEY_F4:
        if( ( dio_oe[ 6 ] & ( 1 << 3 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 3 );
            }
        break;
    case GLUT_KEY_F5:
        if( ( dio_oe[ 6 ] & ( 1 << 4 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 4 );
            }
        break;
    case GLUT_KEY_F6:
        if( ( dio_oe[ 6 ] & ( 1 << 5 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 5 );
            }
        break;
    case GLUT_KEY_F7:
        if( ( dio_oe[ 6 ] & ( 1 << 6 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 6 );
            }
        break;
    case GLUT_KEY_F8:
        if( ( dio_oe[ 6 ] & ( 1 << 7 ) ) == 0 )
            {
            dio[ 6 ] |= ( 1 << 7 );
            }
        break;

    /*------------------------------------------------------
    Default
    ------------------------------------------------------*/
    default:
        break;
    }

}   /* special_cb() */


/*********************************************************************
*
*   time_proc - Periodic callback
*
*********************************************************************/

static void CALLBACK time_proc
    (
    UINT uID,
    UINT uMsg,
    DWORD dwUser,
    DWORD dw1,
    DWORD dw2
    )
{

#define DASH_RATE ( 100 )

static uint32           dash_t = 0;
static uint16           pkt_idx = 0;

if( running )
    {
    time_ms += PRDC_RATE_MS;
    rob_prdc( );
    }
    
if( ( tx_sock != INVALID_SOCKET ) && ( time_ms > dash_t ) )
    {
    dash_t = time_ms + DASH_RATE;
    
    WSABUF              tx_buf;
    dash_pkt.ds.pkt_idx = ntohs( pkt_idx );
    pkt_idx++;
    tx_buf.buf = ( char * )&dash_pkt;
    
    int                 lt;
    int                 rt;

    rob_get_motors( &rt, &lt );    //expect +/- 100
    
    dash_pkt.bot.dig_mod_0.pwm1[ 0 ] = ( 255 * ( lt + 100 ) ) / 200;
    dash_pkt.bot.dig_mod_0.pwm1[ 1 ] = ( 255 * ( rt + 100 ) ) / 200;
    dash_pkt.bot.dig_mod_0.gpio      = ntohs( dio[ 4 ] );
    dash_pkt.bot.dig_mod_1.gpio      = ntohs( dio[ 6 ] );
    dash_pkt.bot.dig_mod_0.gpio_oe   = ntohs( dio_oe[ 4 ] );
    dash_pkt.bot.dig_mod_1.gpio_oe   = ntohs( dio_oe[ 6 ] );
    tx_buf.len = sizeof( dash_pkt );
    WSASendTo
        (
        tx_sock,
        &tx_buf,
        1,
        NULL,
        0,
        ( sockaddr * )&to_addr,
        sizeof( to_addr ),
        &tx_ol,
        &tx_completion );
    }

}   /* time_proc( ) */


/*********************************************************************
*
*   tx_completion - Dashboard tx packet completion callback
*
*********************************************************************/

static void CALLBACK tx_completion
    (
    DWORD               dwError,
    DWORD               cbTransferred,
    LPWSAOVERLAPPED     lpOverlapped,
    DWORD               dwFlags
    )
{
//CDL TODO verify packet

}   /* tx_completion( ) */

